#include <iostream>
#include "d_Math.h"

using namespace d_Math;

d_Mat3f::d_Mat3f()
{
	for(int i=0; i<3; i++) 
		for(int j=0; j<3; j++)
			p[i][j] = 0;
}

d_Mat3f::d_Mat3f(float a00, float a01, float a02, 
		 float a10, float a11, float a12, 
		 float a20, float a21, float a22)
{
	p[0][0]=a00;
	p[0][1]=a01;
	p[0][2]=a02;
	p[1][0]=a10;
	p[1][1]=a11;
	p[1][2]=a12;
	p[2][0]=a20;
	p[2][1]=a21;
	p[2][2]=a22;
}

void d_Mat3f::diagonal(float x)
{
	for(int i=0;i<3;i++)
		p[i][i] = x;
}

void d_Mat3f::print()
{
	std::cout << "[";
	for (int i=0; i<2; i++) 
	{
		std::cout << "[" << p[i][0] << "," << p[i][1] << "," << p[i][2] << "]" << std::endl;
	}
	std::cout << "[" << p[2][0] << "," << p[2][1] << "," << p[2][2] << "]]";
}

float &d_Mat3f::operator () (int ir, int ic)
{ 
	if(ir>3 || ic>3)
	{
		std::cout << "Bad index for matrix!" << std::endl;
		return p[0][0];
	}
	return p[ir][ic]; 
}

float d_Mat3f::operator () (int ir, int ic) const
{ 
	if(ir>3 || ic>3)
	{
		std::cout << "Bad index for matrix!" << std::endl;
		return p[0][0];
	}
	return p[ir][ic]; 
}

d_Vec3f d_Mat3f::operator * (d_Vec3f v)
{
	d_Vec3f result;
	for(int i=0; i<3; i++)
		for(int j=0; j<3; j++)
			result[i] += p[i][j]*v[j];
	return result;
}

d_Mat3f d_Mat3f::operator * (d_Mat3f m)
{
	d_Mat3f res;
	double sum;

	for (int i=0; i<3; i++)
		for (int j=0; j<3; j++){
		    sum = 0;
		    for (int k=0; k<3; k++)
		     	sum+= p[i][k] * m(k,j);        
		         
		    res(i,j) += sum;
		}
	return res;
}

d_Mat3f d_Mat3f::operator / (float x)
{
	d_Mat3f res=(*this);
	for(int i=0; i<3; i++)
		for(int j=0; j<3; j++)
			res(i,j)/=x;
	return res;
}

void d_Mat3f::operator *= (d_Mat3f m)
{
	d_Mat3f res;
	double sum;

	for (int i=0; i<3; i++)
		for (int j=0; j<3; j++){
		    sum = 0;
		    for (int k=0; k<3; k++)
		     	sum+= p[i][k] * m(k,j);        
		         
		    res(i,j) += sum;
		}
	*this = res;

}



